#!/usr/bin/env python
# -*- coding: utf-8 -*-
# Software License Agreement (BSD License)
#
# Copyright (c) 2008, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of Willow Garage, Inc. nor the names of its
#    contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Revision $Id$

## Simple talker demo that published std_msgs/Strings messages
## to the 'chatter' topic
VERSION = 'v2.4.1'

import os
import sys
cwd = os.path.dirname(os.path.abspath(__file__))
sys.path.append(cwd)

import websocket
import rospy
from std_msgs.msg import String, ByteMultiArray, UInt8MultiArray, Byte, Int16MultiArray, Int32MultiArray, Float32MultiArray
import threading
import time
import uuid
import yaml
import threading
from geometry_msgs.msg import Pose, PoseArray

try:
    import thread
except ImportError:  # TODO use Threading instead of _thread in python3
    import _thread as thread

import json
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist, PoseStamped
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal, MoveBaseActionResult, MoveBaseActionGoal
from config import server_ip, robot_id, getVersion, get_robot_id
#from config import get_robot_id, server_ip, robot_id, getVersion
from compute import gzip_uncompress, gzip_compress, splitStr

beatCount = 0
# server port
server_port = rospy.get_param("/clientport/webcomm_param/server_port")
serverIP = "ws://" + server_ip + ":" + str(server_port)
port = rospy.get_param("/clientport/webcomm_param/socket_port")

# file send list
file_list = []
# file uuid(str)
file_uuid = str(uuid.uuid1())
local_ip="192.168.5.103"
local_id="12345678910"
def get_local_info():
    global local_ip
    global local_id
    local_ip = get_robot_id("IP")
    local_id = get_robot_id("MAC")
    while not local_ip:
        if rospy.is_shutdown():
            break
        time.sleep(1)
        local_ip = get_robot_id("IP")
        local_id = get_robot_id("MAC")


#local_info = threading.Thread(target=get_local_info, name="LocalInfo")
#local_info.start()
#local_info.join()
res = get_robot_id("IP")
if res is not None:
    local_ip = res

# websocket to server
def on_message(ws, message):
    if message[0] == "{":
        msg = json.loads(message)
        onMsg(ws, msg)
    elif message[0:3] == "|||":
        onFile(ws, message)
    return


# global openState
openState = False
robot_charge = None
last_pose = {}
last_pda_msg = ['pda_cancel', 0]


# server send file
def onFile(ws, message):
    import base64
    global file_list
    global file_uuid
    data = message.split('|||')
    hearder = json.loads(data[1])
    _body = json.loads(data[2])['data']['data']
    if hearder['type'] == 'map_file':
        body = str(_body)
        file_list.append(body)
        rospy.loginfo('[clientport] server send file len:{} num:{}'.format(hearder['len'] - 1, hearder['num']))
        if hearder['num'] == hearder['len'] - 1:
            if len(file_list) < hearder['len']:
                print ('[clientport] get file false')
                file_list = []
                return
            base_data = ''.join(file_list)
            un_data = gzip_uncompress(base64.b64decode(base_data))
            filetransfer_up_pub.publish(un_data)
            file_uuid = json.loads(un_data)["ID"]
            print ('[clientport] get file success')
            file_list = []
            return


def onMsg(ws, msg):
    global beatCount
    global serverIP
    global robot_charge
    global last_pda_msg
    global tablet_cancel_t
    if msg.has_key('device'):
        if msg["device"] == "robot":
            if msg["topic"] == "target":
                sendtarge(msg)
            elif msg["topic"] == "heartbeat":
                beatCount = 0
            elif msg["topic"] == "get_charge":
                charge_data = "get_battery_level"
                mb_control_pub.publish(charge_data)
            elif msg["topic"] == "move_single":
                if isinstance(msg["data"], str):
                    move_goal = msg["data"]
                else:
                    move_goal = str(msg["data"])
                rospy.loginfo("[clientport] server send move_single: %s", move_goal)
                move_data = "move:" + move_goal
                position_server_pub.publish(move_data)
            elif msg["topic"] == "charge_single":
                charge_cmd = msg["data"]
                rospy.loginfo("[clientport] server send charge_single: %s", str(charge_cmd))
                if isinstance(charge_cmd["id"], str):
                    charge_goal = charge_cmd["id"]
                else:
                    charge_goal = str(charge_cmd["id"])
                charge_data = charge_cmd["type"] + ":" + charge_goal
                position_server_pub.publish(charge_data)

            elif msg["topic"] == "position_server":
                position_server_pub.publish(msg["data"])

            elif msg["topic"] == "manage_error":
                manage_error = msg["data"]
                rospy.loginfo("[clientport] server send manage_error: %s", str(manage_error))
                wsSend("manage_error", "")
                manage_type = manage_error["type"]
                if manage_type == "mission_normalize":
                    position_server_pub.publish(manage_type)
                elif manage_type == "back_zero":
                    zero_data = "error_action:back_zero"
                    position_server_pub.publish(zero_data)
            elif msg["topic"] == "stop_charge":
                pub_cmdbash.publish("CMD_StopCharge")
                rospy.loginfo("[clientport] server send robot CMD_StopCharge ")
            elif msg["topic"] == "get_presentmapname":
                map_cmd = "CMD_GETPRESENTMAPNAME"
                pub_cmdbash.publish(map_cmd)
            elif msg["topic"] == "get_maplist":
                list_cmd = "CMD_GETMAPLIST"
                pub_cmdbash.publish(list_cmd)
            elif msg["topic"] == "change_map":
                map_name = msg["data"]
                mdata = "CMD_CHANGEMAP," + map_name
                pub_cmdbash.publish(mdata)
            elif msg['topic'] == 'robot_version':
                rospy.loginfo ('[clientport] server get robot version')
                pub_cmdbash.publish('CMD_VERSION')
            elif msg["topic"] == "emergent_state":
                rospy.loginfo("[clientport] server get emergent_state")
                mb_control_pub.publish("get_emergent_state")
            elif msg["topic"] == "colide_state":
                rospy.loginfo("[clientport] server get colide_state")
                mb_control_pub.publish("get_colide_state")
            elif msg["topic"] == "task_send":
                rospy.loginfo("[clientport] get task_send data %s!!!", msg["data"]["taskID"])
                wsSend("task_send", "success")
                # cancel_chargeStatus()
                ldata = json.dumps(msg["data"])
                server_task_pub.publish(ldata)
            elif msg["topic"] == "road_status":
                cross_status_data = msg["data"]
                if cross_status_data:
                    rospy.loginfo('[clientport] cross:GO!')
                    cross_is_stopped_pub.publish("go")
                else:
                    rospy.loginfo('[clientport] cross:Stop!')
                    cross_is_stopped_pub.publish("stop")
            elif msg["topic"] == "cross_lock_back":
                cross_lock_data = msg["data"]
                rospy.loginfo('[clientport] cross_lock_back:%s',cross_lock_data)
                if cross_lock_data:
                    rospy.loginfo('[clientport] cross_lock_back:success!')
                    cross_lock_pub.publish("lock_success")

                else:
                    rospy.loginfo('[clientport] cross_lock_back:fail!')
                    cross_lock_pub.publish("lock_fail")
            elif msg["topic"] == "cross_unlock_back":
                cross_unlock_data = msg["data"]
                rospy.loginfo('[clientport] cross_unlock_back:%s',cross_unlock_data)
                if cross_unlock_data:
                    rospy.loginfo('[clientport] cross_unlock_back:success!')
                    cross_lock_pub.publish("unlock_success")

                else:
                    rospy.loginfo('[clientport] cross_unlock_back:fail!')
                    cross_lock_pub.publish("unlock_fail")
            elif msg["topic"] == "cross_appoint_point_back":
                cross_appoint_point = msg["data"]
                rospy.loginfo("[clientport] cross_appoint_point_back: %s",cross_appoint_point)
                if cross_appoint_point=='go':
                    rospy.loginfo('[clientport] cross_appoint_point_back:go!')
                    cross_appoint_point.publish("go")
                else:
                    rospy.loginfo('[clientport] cross_appoint_point_back:stop!')
                    cross_appoint_point.publish("stop")

            elif msg["topic"] == "gocharge":
                rospy.loginfo("[clientport] gocharge!")
                sendtarge(msg)
            elif msg["topic"] == "exchange":
                exchange_data = msg["data"]
                if exchange_data["content"] == "lock" or exchange_data["content"] == "unlock":
                    if exchange_data["lock"]:
                        exchange_data["lock"] = "success"
                    else:
                        exchange_data["lock"] = "false"
                exchange = exchange_data["type"] + ":" + str(exchange_data["id"]) + ":" + exchange_data[
                    "content"] + ":" + str(exchange_data["lock"])
                position_server_pub.publish(exchange)
            elif msg["topic"] == "data_io":
                rospy.loginfo('[clientport] ### get robot IO data ####' + message)
                aa = json.loads(msg["data"])
                mByteMulti = UInt8MultiArray()
                bb = tuple(aa)
                mByteMulti.data = bb
                wireless_data_down_pub.publish(mByteMulti)
            elif msg["topic"] == "getpositions":
                print("[clientport] ### get robot getpositions ###")
                mdata = json.loads(msg["data"])
                rospy.loginfo(json.dumps(mdata, sort_keys=False))
                print(len(mdata))
                count = 0
                for pose in mdata:
                    mstr = ''
                    count += 1
                    mstr = str(mdata[pose]["nick"])
                    mstr += ',' + str(mdata[pose]["x"])
                    mstr += ',' + str(mdata[pose]["y"])
                    mstr += ',' + str(mdata[pose]["a"])
                    mstr += ',' + str(mdata[pose]["w"])
                    pub_servercmd.publish(mstr)
                    rospy.loginfo(mstr)
                    rospy.loginfo("count " + str(count))
            elif msg["topic"] == "cancel":
                rospy.loginfo("[clientport] ### get mission cancel robot ###")
                mission_control_pub.publish("mission_cancel")
                # pub_cmdbash.publish("CMD_CANCELGOAL")
                state_data = {'running': 0}
                wsSend('state', state_data)
                time.sleep(0.1)
            elif msg["topic"] == "mission_control":
                rospy.loginfo("[clientport] ### get mission_control %s ###", msg["data"])
                mission_msg = msg["data"]
                mission_control_pub.publish(mission_msg)
            elif msg["topic"] == "music_control":
                rospy.loginfo("[clientport] get music_control data %s", msg["data"])
                music_cmd = msg["data"]
                music_control_pub.publish(music_cmd)
            elif msg["topic"] == "server_task_message":


                if 'pda' in msg['data']:
                    data = msg['data'].split(':')

                    if time.time()-tablet_cancel_t<5 and data[1] == last_pda_msg[1] and data[0] == last_pda_msg[0]:
                        return

                    if data[0] == 'pda' and last_pda_msg[0] == 'pda' and data[1] != last_pda_msg[1]:
                        msg['data'] = 'pda_cancel:' + last_pda_msg[1]
                    last_pda_msg = data
                    
                rospy.loginfo("[clientport] get server_task_message data %s", msg["data"])
                server_task_message.publish(msg["data"])
            elif msg["topic"] == "robots_position":
                # rospy.loginfo("[clientport] ### get robots_positions %s ###", msg['data'])
                robots_positions.publish(msg["data"])
                # wsSend('robots_positions', '')
            elif msg['topic'] == 'jack_up':
                rospy.loginfo("[clientport] ### get jack_up %s ###", msg['data'])
                jack_up.publish(msg["data"])
                wsSend('jack_up', '')
            elif msg['topic'] == 'server_time_now':
                server_time_now.publish(msg["data"])
                wsSend('server_time_now', '')
            elif msg['topic'] == 'cross_v2_response':
                rospy.loginfo("[clientport] cross_v2_response: %s", msg['data'])
                cross_v2_response.publish(msg["data"])
                wsSend('cross_v2_response', '')
            elif msg['topic'] == 'errcode_report':
                rospy.loginfo("[clientport] errcode_report: %s", msg['data'])
                errcode_report.publish(msg["data"])
                wsSend('errcode_report', '')
            elif msg['topic'] == 'error_code_back':
                rospy.loginfo("[clientport] error_code_back: %s", msg['data'])
                error_code_back.publish(msg["data"])
                wsSend('error_code_back', '')
            
    return


def on_error(ws, error):
    rospy.logerr("[clientport] ws error ",error)
    # global openState
    # openState = False
    # ws.close()


def on_close(ws):
    rospy.logwarn("[clientport] ### ws closed ###")
    # global openState
    # openState = False
    # ws.close()

def on_open(ws):
    rospy.loginfo("[clientport] ### webService  opened ###################################")
    global openState
    global beatCount
    global local_id
    global local_ip
    global obstacle_status
    import time
    openState = True
    data = {'psd': 1990, 'MAC': local_id, "IP": local_ip}
    wsSend('login', data)
    rospy.loginfo("[clientport] " + json.dumps(data, sort_keys=False))
    beatCount = 0
    mb_control_pub.publish("get_emergent_state")

    if obstacle_status != '':
        wsSend("exception_info", exception_data)

    def run(*args):
        global openState
        while openState:
            time.sleep(1)
        time.sleep(1)
        ws.close()
        # ws.on_close = on_close
        # openState = False
        rospy.logwarn("[clientport] Thread terminating...")

    thread.start_new_thread(run, ())
    return


pos_count = 0



def get_time_stamp():
    ct = time.time()
    local_time = time.localtime(ct)
    data_head = time.strftime("%Y-%m-%d %H:%M:%S", local_time)
    data_secs = (ct - long(ct)) * 1000
    time_stamp = "%s.%03d" % (data_head, data_secs)
    return time_stamp


# robot pos
def pos_callback(data):
    global pos_count
    global openState
    global last_pose
    if pos_count > 9:
        poseData = {'x': 0, 'y': 0, 'a': 0, 'w': 1}
        poseData["x"] = data.position.x
        poseData["y"] = data.position.y
        poseData["a"] = data.orientation.z
        poseData["w"] = data.orientation.w
        last_pose = poseData
        if openState:
            wsSend('pose', poseData)
        pos_count = 0

    else:
        pos_count += 1
    return


def res_cmdRev(data):
    # rospy.loginfo(rospy.get_caller_id() + 'I heard CMD_BASH %s ', data.data)
    # 平板返航信息
    if data.data == 'CMD_SERVERGOBACK':
        data = {'cmd': 1}
        wsSend('goback', data)
    elif data.data == 'CMD_SERVERPOSITIONS':
        data = {'cmd': 1}
        wsSend('getpositions', data)

    return


def res_callback(data):
    rospy.logwarn("[clientport] /move_base/result: caller_id:" + rospy.get_caller_id() + ', text:%s, status:%d', data.status.text, data.status.status)
    status = {'status': data.status.status}
    wsSend("status", status)
    return


# 发送信息到server
def wsSend(topic, data):
    global openState
    try:
        if openState:
            robot_data = {'device': 'robot', 'topic': topic, "robotID": robot_id, 'data': data}
            ws.send(json.dumps(robot_data, sort_keys=False).encode('utf-8'))
        else:
            rospy.logwarn("[clientport] ### ws is not open ###")
            # ws.close()
    except BaseException, e:
        rospy.logwarn(e)
    return


# 发送移动和充电坐标
def sendtarge(msg):
    pose_stamped = PoseStamped()
    mdata = msg["data"]
    rospy.loginfo('[clientport] I get  pos %f,%f,%f,%f', mdata["x"], mdata["y"], mdata["z"], mdata["w"])
    pose_stamped.pose.position.x = mdata["x"]
    pose_stamped.pose.position.y = mdata["y"]
    pose_stamped.pose.position.z = 0.00
    pose_stamped.pose.orientation.z = mdata["z"]
    pose_stamped.pose.orientation.w = mdata["w"]
    pose_stamped.pose.orientation.x = 0.00
    pose_stamped.pose.orientation.y = 0.00

    if msg["topic"] == "target":
        rospy.loginfo('[clientport] I go to target!!!')
        pose_stamped.header.frame_id = "/map"
        cancel_chargeStatus()
        move_base_simple_pub.publish(pose_stamped)
    elif msg["topic"] == "gocharge":
        rospy.loginfo('[clientport] I go to charge!!!')
        pose_stamped.header.frame_id = ""
        charge_position_pub.publish(pose_stamped)
    data = {'running': 1}
    wsSend('state', data)

    rospy.loginfo('[clientport] I heard result running: 1----')

    return


# 接收底层串口信息
def WirelessDataUpCB(msg):
    rospy.loginfo('[clientport] I get IO cmd')
    array = []
    for i in msg.data:
        array.append(i)
    wsSend('dataIO', array)
    return


# 任务链信息
def position_server_callback(msg):
    global robot_state_msg
    rospy.loginfo('[clientport] i get task_receive data %s', msg.data)
    data = msg.data
    try:
        msg_data = data.split(':')
        if msg_data[0] == "task":
            wsSend('task_receive', msg_data[1])
        elif msg_data[0] == "robot_status":
            wsSend("robot_status", msg_data[1])
        elif msg_data[0] == "move_back" or msg_data[0] == "move_result":
            send_data={"id": msg_data[1], "content": msg_data[2]}
            wsSend('move_single', send_data)
        elif msg_data[0] == "charge_back" or msg_data[0] == "charge_result":
            send_data = {"id": msg_data[1], "content": msg_data[2]}
            wsSend('charge_single', send_data)
        elif msg_data[0] == "6250_control":
            control_ip = msg_data[1]
            control_data = msg_data[2]
            set_do_data('setDO', control_ip, control_data)
        elif msg_data[0] == "6250_status":
            if msg_data[2] == "get_di_status":
                di_ip = msg_data[1]
                get_di_data('getDI', di_ip)
        elif msg_data[0] == "error_action":
            if msg_data[1] == "back_zero":
                wsSend("back_zero", msg_data[2])
        elif msg_data[0] == "mission_continue" or msg_data[0] == "mission_pause":
            mission_data = {"type": msg_data[0], "id": msg_data[1], "content": msg_data[2]}
            wsSend("mission_control", mission_data)
        elif msg_data[0] == "equipment_lock":
            exchange_data = {"type": msg_data[0], "id": msg_data[1], "content": msg_data[2]}
            wsSend("exchange", exchange_data)
        elif msg_data[0] == "music_control":
            music_data = {"type": msg_data[1], "content": msg_data[2]}
            wsSend("music_control", music_data)
        elif msg_data[0] == "Hmi_control":
            hmi_data = msg_data[1].split(",")
            if hmi_data[2] == "hmi":
                address = hmi_data[0] + ":" + hmi_data[1]
                count = hmi_data[4]
                if hmi_data[3] == "set":
                    state = hmi_data[5]
                    setHmi("setHmi", address, count, state)
                elif hmi_data[3] == "get":
                    getHmi("getHmi", address, count)
        else:
            wsSend("position_server_send", data)

    except Exception, err:
        print("position_server_callback:", err)

    if robot_state_msg != '':
        wsSend("exception_info", {"type": 'moving_state', "content": robot_state_msg})

    return


# 取消充电状态
def cancel_chargeStatus():
    global robot_charge
    if robot_charge == "charge_success" or robot_charge == 'go_to_charge':
        pub_cmdbash.publish("CMD_StopCharge")
        rospy.loginfo("[clientport] send robot CMD_StopCharge %s", robot_charge)
        time.sleep(6)
    return


# 控制电梯
def set_do_data(key, ip, doData):
    import socket
    try:
        s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        s.connect((server_ip, port))
    except socket.error as msg:
        print(msg)
    print("[clientport] " + s.recv(1024))
    s.send(key)
    sta_cmd = s.recv(1024)
    print ("[clientport] " + sta_cmd)
    s.send(ip)
    print("[clientport] " + s.recv(1024))
    s.send(doData)
    str_result = s.recv(1024)
    print("[clientport] " + str_result)
    s.close()
    set_cmd = "6250_control:" + ip + ":" + str_result
    position_server_pub.publish(set_cmd)
    print ("[clientport] " + set_cmd)
    print('[clientport] over')
    return


# 获取电梯状态
def get_di_data(key, ip):
    import socket
    try:
        s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        s.connect((server_ip, port))
    except socket.error as msg:
        print(msg)
    print("[clientport]" + s.recv(1024))
    s.send(key)
    print("[clientport] " + s.recv(1024))
    s.send(ip)
    sta_cmd = s.recv(1024)
    print("[clientport] " + sta_cmd)
    s.close()
    get_cmd = "6250_status:" + ip + ":" + sta_cmd
    position_server_pub.publish(get_cmd)
    print ("[clientport] " + get_cmd)
    print('[clientport] over')
    return


# 设置hmi
def setHmi(key, address, count, state):
    import socket
    try:
        s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        s.connect((server_ip, port))
    except socket.error as msg:
        print(msg)
    print("[clientport] " + s.recv(1024))
    s.send(key)
    sta_cmd = s.recv(1024)
    print ("[clientport] " + sta_cmd)
    print (address)
    s.send(address)
    print("[clientport] " + s.recv(1024))
    doData = count + ":" + state
    s.send(doData)
    str_result = s.recv(1024)
    print("[clientport] " + str_result)
    s.close()
    print('[clientport] over')
    return


# 获取hmi状态
def getHmi(key, address, count):
    import socket
    try:
        s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        s.connect((server_ip, port))
    except socket.error as msg:
        print(msg)
    print("[clientport] " + s.recv(1024))
    s.send(key)
    print("[clientport] " + s.recv(1024))
    s.send(address)
    print("[clientport] " + s.recv(1024))
    s.send(count)
    hmi_status = s.recv(1024)
    print (hmi_status)
    s.close()
    get_cmd = "Hmi_status:" + count + ":" + hmi_status
    position_server_pub.publish(get_cmd)
    print ("[clientport] " + get_cmd)
    print('[clientport] over')
    return


# 异常信息

move_state = False
obstacle_status = ''

def exception_info_callback(msg):
    data = msg.data.split(",")
    global move_state
    try:
        if len(data) < 2:
            wsSend("exception_info", data)
        else:
            exception_data = {"type": data[0], "content": data[1]}
            if data[0] == "moving_state":
                wsSend("exception_info", exception_data)
                rospy.loginfo("[clientport] moving_state: %s", data[1])
                if data[1] == "no_obstacle" or data[1] == "cross_stop":
                    obstacle_status = data[1]
                    move_state = False
                elif data[1] == "has_obstacle" or data[1] == "obstacle_stop":
                    obstacle_status = data[1]
                    move_state = True
            elif data[0] == "battery_level_low" or data[0] == "battery_level_badly_low":
                exception_data["type"] = "charge_warn"
                wsSend("exception_info", exception_data)
                rospy.loginfo("[clientport] charge_warn: %s", data[1])
    except AttributeError, e:
        rospy.logwarn("[clientport] exception_info err: %s", e.message)

    return


def info_back_callback(msg):
    try:
        info_data = msg.data.split(":")
        send_data = info_data[-1].replace(" ", "")
        # 当前地图名
        if info_data[0] == "map_name":
            wsSend("get_presentmapname", send_data)
        #  切换地图
        elif info_data[0] == "CMD_CHANGEMAP":
            wsSend("change_map", send_data)
        # 切换地图返回值
        elif info_data[0] == "CMD_CHANGEMAP,[1]" or info_data[0] == "CMD_CHANGEMAP,[2]" or info_data[
            0] == "CMD_CHANGEMAP,[3]":
            wsSend("change_map", msg.data)
        # 小车版本信息
        elif info_data[0] == "CMD_VERSION":
            send_version = getVersion(msg)
            wsSend('robot_version', send_version)

    except Exception, err:
        print("info_back_callback:",err)
    return


# 地图列表
def maplist_callback(msg):
    rospy.loginfo("[clientport] get maplist data")
    maplist = msg.data
    wsSend("get_maplist", maplist)
    return


# global vel_num
vel_num = 0


# 小车速度
def vel_mb_callback(msg):
    global openState
    global vel_num
    if vel_num == 90:
        speed = {'linear': msg.linear.x, 'angle': msg.angular.z}
        if openState:
            wsSend('robot_speed', speed)
        vel_num = 0
    vel_num += 1

    return


# 交管信息
def cross_path_info(msg):
    global roads_plan
    roads_plan = rospy.get_param("/clientport/webcomm_param/roads")
    # if not roads_plan:
    #     roads_plan = rospy.get_param("/clientport/webcomm_param/roads")

    cross_num = msg.layout.data_offset
    cross_data = msg.data
    try:
        cross_arr = []
        roads = []
        send_arr = []
        cross_pos = cross_data[:cross_num]
        if len(cross_data) == 1:
            send_arr = [{"cross_id":cross_data[0], "cross_data":[]}]
        else:
            for i in cross_pos:
                cross_arr.append(i)
            paths_len = len(cross_data)
            paths = [cross_data[i:i + 3] for i in range(cross_num, paths_len, 3)]
            for p in paths:
                path_old = {"id": p[0], "start": p[1], "end": p[2]}
                roads.append(path_old)
            for index, val in enumerate(cross_arr):
                if len(roads):
                    send_data = {"cross_id": val, "cross_data": [roads[0]]}
                    roads.pop(0)
                else:
                    send_data = {"cross_id": val, "cross_data": []}
                    break
                if index + 1 < len(cross_arr):
                    t = index + 1
                else:
                    t = index
                for arr in range(len(roads)):
                    if cross_arr[t] == roads[0]["start"]:
                        break
                    else:
                        send_data["cross_data"].append(roads[0])
                        roads.pop(0)
                send_arr.append(send_data)
        if roads_plan == "None":
            plan_data = "None"
        elif isinstance(roads_plan,str):
            plan_data = json.loads(roads_plan)
        else:
            plan_data = roads_plan
        json_data = {"roads_plan": plan_data, "roads": send_arr}
        wsSend("cross_status", json_data)
        rospy.loginfo('[clientport] %s cross_status:%s', get_time_stamp(), str(json_data))
    except Exception, err:
        rospy.logwarn("[clientport] cross_path err %s", err)
        return

def cross_lock_point_info(msg):
    cross_lock_data = msg.data
    try:
        point_arr=[]
        for point in cross_lock_data:
            point_arr.append(point)
        wsSend("cross_lock_point",point_arr)
        rospy.loginfo("[clientport] cross_lock_point_info: %s",point_arr)

    except Exception, err:
        rospy.logwarn("[clientport] cross_lock_point_info err %s", err)
        return

def cross_appoint_point_info(msg):
    cross_appoint_point_data = msg.data
    try:
        point_arr=[]
        for point in cross_appoint_point_data:
            point_arr.append(point)
        wsSend("cross_appoint_point",point_arr)
        rospy.loginfo("[clientport] cross_appoint_point_info: %s",point_arr)

    except Exception, err:
        rospy.logwarn("[clientport] cross_appoint_point_info err %s", err)
        return


# 转发道路解锁信息
def cross_unlock_point_info(msg):
    cross_unlock_data = msg.data
    try:
        points=[]
        for p in cross_unlock_data:
            points.append(p)
        wsSend("cross_unlock_point",points)
        rospy.loginfo("[clientport] cross_unlock_point_info: %s",points)

    except Exception, err:
        rospy.logwarn("[clientport] cross_unlock_point_info err %s", err)
        return

# 状态数据返回
emergent_state = 'no'
def mb_data_callback(msg):
    global robot_charge
    global emergent_state
    data = msg.data.split(',')
    # 充电状态
    if msg.data == " battery_info,full" or msg.data == "battery_info,low":
        return
    elif data[0] == 'charge_state':
        wsSend('charge_state', data[1])
        if data[1] == 'connect':
            robot_charge = 'charge_success'
    elif data[0] == "battery_level":
        wsSend("get_charge", data[1])
        rospy.loginfo('[clientport] charge_num: %s', data[1])
    elif data[0] == "charge_success" or data[0] == "charge_failed" or data[0] == "charge_finish" or data[0] == "go_to_charge" or data[0] == "charge_finish_warnning":
        robot_charge = msg.data
        wsSend("dsp_control", robot_charge)
        rospy.loginfo('[clientport] charge_status: %s', robot_charge)
    elif data[0] == "stop_charge_success" or data[0] == "stop_charge_warnning":
        wsSend("stop_charge", data[0])
        rospy.loginfo('[clientport] stop_charge: %s', data[0])
    elif data[0] == "emergent":
        rospy.loginfo('[clientport] emergent_state: %s', data[1])
        emergent_state = data[1]
        wsSend("emergent_state", data[1])
    elif data[0] == "colide":
        wsSend("colide_state", data[1])

    # rospy.loginfo("[clientport] robot_charge: %s", robot_charge)
    return


roads_plan = None


# 规划道路
def road_plan_info(msg):
    global roads_plan
    data = json.loads(msg.data)
    roads_plan = json.dumps(data["roads"])
    rospy.loginfo('[clientport] get road plan data %s !', roads_plan)
    rospy.set_param('/clientport/webcomm_param/roads', roads_plan)

    wsSend("road_plan", data)
    return


def present_road_info(msg):
    # rospy.loginfo("[clientport] get prement road %s!!!", str(msg.data))
    # rospy.loginfo (msg.data)
    wsSend("road_present", json.loads(msg.data))


def wlan_signal_info(msg):
    wsSend('wlan_signal', json.loads(msg.data))


def fridgecode_info(msg):
    rospy.loginfo("[clientport] fridgecode %s!!!", str(msg))
    wsSend('fridgecode', str(msg))


tablet_cancel_t = 0

def local_task_message(msg):
    global tablet_cancel_t

    data = str(msg.data)
    rospy.loginfo("[clientport] local_task_message %s!!!", data)
    wsSend('local_task_message', data)

    if data == 'tablet_cancel':
        tablet_cancel_t = time.time()

    # if 'tablet_move:' in data:
    #     for i in range(2):
    #         time.sleep(1)
    #         wsSend('local_task_message', data)

# webcomm_open = None

last_robot_state_time = 0
robot_state_msg = ''
def robot_state(msg):
    global robot_state_msg
    global last_robot_state_time
    global emergent_state

    new_robot_state_msg = ''
    if msg.data == 2: new_robot_state_msg = 'obstacle_stop'
    elif msg.data == 3: new_robot_state_msg = 'no_obstacle'

    if time.time() - last_robot_state_time > 60 or robot_state_msg != new_robot_state_msg:
        rospy.loginfo("[clientport] robot_state %s!!!", str(robot_state_msg))

    if time.time() - last_robot_state_time > 10 or robot_state_msg != new_robot_state_msg:
        last_robot_state_time = time.time()
        robot_state_msg = new_robot_state_msg
        wsSend("exception_info", {"type": 'moving_state', "content": robot_state_msg})
        wsSend("emergent_state", emergent_state)

def move_base_callback(msg):
    x = msg.goal.target_pose.pose.position.x
    y = msg.goal.target_pose.pose.position.y
    z = msg.goal.target_pose.pose.orientation.z
    w = msg.goal.target_pose.pose.orientation.w

    wsSend("move_base_callback", {'x':x,'y':y,'z':z,'w':w})
    rospy.loginfo("[clientport] last_pose %s!!!", json.dumps(last_pose))
    rospy.loginfo("[clientport] move_base_callback %s!!!", json.dumps({'x':x,'y':y,'z':z,'w':w}))
    

def move_base_simple_callback(msg):
    x = msg.pose.position.x
    y = msg.pose.position.y
    z = msg.pose.orientation.z
    w = msg.pose.orientation.w

    wsSend("move_base_simple_callback", {'x':x,'y':y,'z':z,'w':w})
    rospy.loginfo("[clientport] last_pose %s!!!", json.dumps(last_pose))
    rospy.loginfo("[clientport] move_base_simple_callback %s!!!", json.dumps({'x':x,'y':y,'z':z,'w':w}))

def move_base_cancel_callback(msg):
    rospy.loginfo("[clientport] move_base_cancel_callback: %s", str(msg))

# 检测服务器ip是否可以连接
def ping_server():
    backinfo = 0
    while backinfo:
        try:
            if rospy.is_shutdown():
                rospy.logwarn("[clientprot] ping_server rospy is_shutdown")
                break
            time.sleep(0.1)
            rospy.logwarn("[clientprot] server is not open!!")
            backinfo = os.system('ping -c 1 -w 1 %s' % server_ip)  # 实现pingIP地址的功能，-c1指发送报文一次，-w1指等待1秒
        except Exception, err:
            rospy.logwarn("[clientport] ping_server err %s", err)
            return
    return

def cross_v2_request(msg):
    rospy.loginfo('[clientport] cross_v2_request: %s' % msg.data)
    wsSend("cross_v2_request", msg.data)

def speed_control_status(msg):
    wsSend("speed_control_status", msg.data)

def mb_test_callback(msg):
    rospy.loginfo('[clientport] mb_test: %s' % msg.data)

def jack_up_callback(msg):
    rospy.loginfo('[clientport] jack_up: %s' % msg.data)

obstacle_avoidance_t = time.time()
def obstacle_avoidance_callback(msg):
    global obstacle_avoidance_t
    if time.time()-obstacle_avoidance_t>0.3:
        wsSend("obstacle_avoidance", msg.data)
        obstacle_avoidance_t = time.time()

pub_readlist_t = time.time()
def pub_readlist_callback(msg):
    global pub_readlist_t

    try :
        if time.time()-pub_readlist_t>0.3:
            data = msg.data.split(',')
            if len(data)>=4:
                wsSend("pub_readlist", data[3])
                pub_readlist_t = time.time()
    
    except Exception, err:
        rospy.logwarn("[clientport] pub_readlist_callback err %s", err)
        return

obstacle_distance_t = time.time()
def obstacle_distance_callback(msg):
    global obstacle_distance_t

    try :
        if time.time()-obstacle_distance_t>0.3:
            wsSend("obstacle_distance", msg.data)
            obstacle_distance_t = time.time()
    
    except Exception, err:
        rospy.logwarn("[clientport] pub_readlist_callback err %s", err)
        return
        
cmd_vel_mb_t = time.time()
def cmd_vel_mb_callback(msg):
    global cmd_vel_mb_t

    try :
        if time.time()-cmd_vel_mb_t>0.3:
            wsSend("cmd_vel_mb", msg.linear.x)
            cmd_vel_mb_t = time.time()
    
    except Exception, err:
        rospy.logwarn("[clientport] cmd_vel_mb_t_callback err %s", err)
        return

vel_actual_t = time.time()
def vel_actual_callback(msg):
    global vel_actual_t

    try :
        if time.time()-vel_actual_t>0.3:
            wsSend("vel_actual", msg.linear.x)
            vel_actual_t = time.time()
    
    except Exception, err:
        rospy.logwarn("[clientport] vel_actual_t_callback err %s", err)
        return

def ros_to_agent_callback(msg):
    wsSend("ros_to_agent", json.loads(msg.data))
    
def robot_status_callback(msg):
    # rospy.loginfo('[clientport] robot_status_send : %s' % msg.data)
    wsSend("robot_status_interval", msg.data)
    return


def error_code_callback(msg):
    rospy.loginfo('[clientport] error_code_send : %s' % msg.data)
    wsSend("error_code_send", msg.data)

    return
def path_id_series(data):
    global roads_plan

    res = yaml.load(str(data))
    poses = res["poses"]
    road_arr = []
    for p in poses:
        road_id = p["orientation"]["w"]
        data_id = int(road_id)
        road_arr.append(data_id)

    rospy.set_param('/clientport/webcomm_param/roads', road_arr)
    send_data = {"marks": [], "roads": road_arr}
    rospy.loginfo("[clientport_road] road_arr:" + str(road_arr))
    
    data = send_data
    roads_plan = data["roads"]
    rospy.loginfo('[clientport] get road plan data %s !', roads_plan)
    rospy.set_param('/clientport/webcomm_param/roads', roads_plan)
    wsSend("road_plan", data)

def create_ws():
    global ws

    ping_server()

    ws = websocket.WebSocketApp(host,
                                on_message=on_message,
                                # on_error=on_error,
                                on_close=on_close)
    rospy.loginfo("[clientport] open websocket %s", host)
    ws.on_open = on_open
    ws.run_forever(ping_interval=60, ping_timeout=10)
    time.sleep(1)
    rospy.loginfo("[clientport] ws close")
    ws.close()
    rospy.logwarn("[clientport] ### ws forever done ###")

def server_bridge():
    global cwd

    while True:
        if os.path.isfile(cwd+'/server_bridge'):
            rospy.loginfo("[clientport] server_bridge start")
            os.system(cwd+'/server_bridge')
        time.sleep(10)

if __name__ == '__main__':
    websocket.enableTrace(False)
    host = serverIP
    rospy.init_node('clientport', "clientport")

    pub = rospy.Publisher('/chatter', String, queue_size=10)
    pub_servercmd = rospy.Publisher('/cmd_server', String, queue_size=10)
    pub_cmdbash = rospy.Publisher('/CMD_BASH', String, queue_size=10)
    cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
    wireless_data_down_pub = rospy.Publisher('/wireless_data_down', UInt8MultiArray, queue_size=10)
    charge_position_pub = rospy.Publisher('/charge_cmd', PoseStamped, queue_size=10)
    position_server_pub = rospy.Publisher('/position_server_receive', String, queue_size=10)
    server_task_pub = rospy.Publisher('/server_task_list', String, queue_size=10)
    move_base_simple_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=10)
    cross_is_stopped_pub = rospy.Publisher('/move_base_node/cross_is_stopped_back', String, queue_size=10)
    filetransfer_up_pub = rospy.Publisher('/filetransfer_up', String, queue_size=10)
    mission_control_pub = rospy.Publisher('/mission_implement', String, queue_size=10)
    mb_control_pub = rospy.Publisher('/mb_data_check', String, queue_size=10)
    music_control_pub = rospy.Publisher('/music_control', String, queue_size=10)
    cross_lock_pub = rospy.Publisher('/cross_lock_back', String, queue_size=10)
    cross_appoint_point = rospy.Publisher('/cross_appoint_point_back', String, queue_size=10)
    server_task_message = rospy.Publisher('/serverTaskMessage', String, queue_size=10)
    robots_positions = rospy.Publisher('/robots_positions', String, queue_size=10)
    jack_up = rospy.Publisher('/jack_up', String, queue_size=10)
    server_time_now = rospy.Publisher('/server_time_now', String, queue_size=10)
    cross_v2_response = rospy.Publisher('/move_base_node/cross_v2_response', String, queue_size=10)
    errcode_report = rospy.Publisher('/errcode_report', String, queue_size=10)
    # 20220816 新增故障手动上报返回
    error_code_back = rospy.Publisher('/errorcode_server_back', String, queue_size=10)

    rospy.Subscriber('/baselink_tf', Pose, pos_callback)
    rospy.Subscriber('/move_base/result', MoveBaseActionResult, res_callback)
    rospy.Subscriber('/CMD_BASH', String, res_cmdRev)
    rospy.Subscriber('/wireless_data_up', ByteMultiArray, WirelessDataUpCB)
    rospy.Subscriber('/position_server_send', String, position_server_callback)
    rospy.Subscriber('/exception_info', String, exception_info_callback)
    rospy.Subscriber('/info_back', String, info_back_callback)
    rospy.Subscriber('/pub_maplist', String, maplist_callback)
    rospy.Subscriber('/vel_mb', Twist, vel_mb_callback)
    # 当前道路信息
    # rospy.Subscriber('/move_base_node/present_path_info', Int32MultiArray, present_path_info)
    # 交管道路信息
    rospy.Subscriber('/move_base_node/cross_path_info', Int32MultiArray, cross_path_info)
    # 申请道路加锁
    rospy.Subscriber('/cross_lock_point',Int32MultiArray,cross_lock_point_info)
    # 申请道路解锁
    rospy.Subscriber('/cross_unlock_point',Int32MultiArray,cross_unlock_point_info)
    # 查询交管点
    rospy.Subscriber('/cross_appoint_point',Int32MultiArray,cross_appoint_point_info)
    # 状态数据返回
    rospy.Subscriber('/mb_data_check_back', String, mb_data_callback)
    # 规划道路
    # rospy.Subscriber('/road_plan', String, road_plan_info)
    # 当前道路
    rospy.Subscriber('/present_road', String, present_road_info)
    # 摄像头读取结果
    rospy.Subscriber('/fridgecode', String, fridgecode_info)
    rospy.Subscriber('/localTaskMessage', String, local_task_message)

    # 当前信息强度
    rospy.Subscriber('/wlan_signal', String, wlan_signal_info)

    rospy.Subscriber('/robot_state', Byte, robot_state)

    rospy.Subscriber('/move_base/goal', MoveBaseActionGoal, move_base_callback)
    rospy.Subscriber('/move_base_simple/goal', PoseStamped, move_base_simple_callback)
    rospy.Subscriber('/move_base/cancel', GoalID, move_base_cancel_callback)

    rospy.Subscriber('/move_base_node/cross_v2_request', String, cross_v2_request)
    rospy.Subscriber('/move_base_node/speed_control_status', String, speed_control_status)
    rospy.Subscriber('/mb_test', String, mb_test_callback)
    rospy.Subscriber('/jack_up', String, jack_up_callback)
    rospy.Subscriber('/obstacle_avoidance', String, obstacle_avoidance_callback)
    rospy.Subscriber('/pub_readlist', String, pub_readlist_callback)
    rospy.Subscriber('/obstacle_distance', Float32MultiArray, obstacle_distance_callback)
    rospy.Subscriber('/cmd_vel_mb', Twist, cmd_vel_mb_callback)
    rospy.Subscriber('/vel_actual', Twist, vel_actual_callback)
    rospy.Subscriber('/ros_to_agent', String, ros_to_agent_callback)
    rospy.Subscriber('/path_id_series', PoseArray, path_id_series)
    # 20220816 新增故障自动上报
    rospy.Subscriber('/robot_status_send', String, robot_status_callback)

    # 20220816 新增故障手动上报
    rospy.Subscriber('/errorcode_server_send', String, error_code_callback)

    rate = rospy.Rate(1)  # 10hz
    move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)

    t = threading.Thread(target=server_bridge)
    t.start()
    
    while not rospy.is_shutdown():
        create_ws()

    rospy.loginfo("[clientport] rospy is_shutdown")
